/*
 * ClimberSubsytem.h
 *
 *  Created on: Feb 3, 2013
 *      Author: N500432
 */
#ifndef _CLIMBER_SUBSYSTEM_H
#define _CLIMBER_SUBSYSTEM_H

#include <WPILib.h>
#include "EntechSubsystem.h"
#include "TilterSubsystem.h"
#include "TimedSolenoid.h"

class ClimberSubsystem : public EntechSubsystem
{
public:
    ClimberSubsystem();
    virtual ~ClimberSubsystem();

    // Subsystem functions
    virtual void DoRobotInit(void);
    virtual void DoDisabledInit(void);
    virtual void DoTeleopInit(void);
    virtual void DoDisabled(void);
    virtual void DoTeleop(void);

    // Specific Commands
    void TeleopInputs(bool deadman, bool leftForw, bool leftBack, bool rightForw, bool rightBack);
    void Stop(void);

private:
    void SetMotors(double left, double right);

    enum ClimbSystemState { kPositioningChains, kHooking1, kClimbing, kDumping, kRetract, kDone };

    // Systems we have control over
    CANJaguar *m_leftMotor;
    CANJaguar *m_rightMotor;
    DigitalInput *m_leftPositionSwitch;
    DigitalInput *m_rightPositionSwitch;
    TilterSubsystem *m_tilter;

    // Internal state variables
    ClimbSystemState m_state;
    bool m_autoDeadman;
    int  m_leftManual;
    int  m_rightManual;
    bool m_leftAtStart;
    bool m_rightAtStart;
    bool m_leftHooked;
    bool m_rightHooked;
    
    // Encoder values when Position switch triggered
    double m_leftEncStart;
	double m_rightEncStart;
	
	float m_leftNoloadCurrent;
	float m_rightNoloadCurrent;
	float m_motorSpeedDelta;
		
	// Numbers are based on 250 counts/rev encoders, 6" dia sprokets
	static const float c_encoderCountsPerInch = 250.0 / (3.1415926 * 6.0);
	
    static const double c_left_inverter = 1.0;
    static const double c_right_inverter = 1.0;
    
    static const double c_climb_speed = 0.90;
    static const double c_manual_climb_speed = 0.50;

    // Encoder numbers for key moments in the climb
	static const double c_encoderFirstHookup    =  0.0;
	static const double c_encoderTilterRetract  = 12.0;
	static const double c_encoderPusherDeploy1  = 24.0;
	static const double c_encoderSecondHookup   = 34.0;
	static const double c_encoderPusherRetract1 = 36.0;
	static const double c_encoderPusherDeploy2  = 65.0;
	static const double c_encoderThirdHookup    = 68.0;
	static const double c_encoderPusherRetract2 = 70.0;
	static const double c_encoderStopClimb      = 72.0;
};

#endif
